#include <avr/io.h>
#include <avr/wdt.h>
#include "i2c.h"
#include "auvlib_usb.h"
#include "a2d12ch.h"


////////////////////////////////////////////////////////////////////
// ICD Stuff

u08 ucStatus = STAT_FUBAR; // default.

// ordinarily, these would be defined as floats, and you may define them that way, if you wish.
u32 fVoltage = 0x40A00000; // 5 V.  This should be updated by an ADC ISR.
u32 fCurrent = 0x3E19999A; // 0.15 A.  This should be updated by an ADC ISR.


////////////////////////////////////////////////////////////////////
// Motor Stuff

// Motor controller addresses
#define I2C_ADDR_VERT 0xB0
#define I2C_ADDR_HORZ 0xB2
#define I2C_ADDR_DRIV 0xB4

volatile u08 stop = 0;
u08 DeadManCounter;
u08 DeadManTimeout;
u08 timerDividerM = 0;
volatile u08 mcRefresh = 0;

u32 i2cresetcnt = 0; // how many times has i2c reset itself?

// Speed settings
s08 vertical  [2] = {0,0};
s08 horizontal[2] = {0,0};
s08 drive     [2] = {0,0};

  u08 present=0x38; // 0x80=motors, 0x40=compass, 0x20=marker dropper, 0x10 depth sensor, 0x08 PMU
//u08 present=0x78; // just assume compass is present for now (it's not)

// Marker dropper functions
void MarkerDropperInit(void);
void SetMarkerPosition(u16 pwmDuty);

void MotorControllersInit(void);
void updateMD22( u08 addr, s08* channels );
u08 setKillMotors( u08 state );

// A 14MHz clock and 256 prescaler means this timeout should run 
// at 225 Hz (overflow on an 8bit timer)

// Divide this down to 28.125Hz (to make deadman timer more sane)
void DeadManTimer(void) {
    if( timerDividerM == 7 ) {
        // Count up the deadman timer
        if(++DeadManCounter > DeadManTimeout ) {
            stop = 1;
            mcRefresh = 1;
            DeadManCounter = 0;
        }
    }

    timerDividerM++;
	timerDividerM&=7;
}


  




////////////////////////////////////////////////////////////////////
// Main

int main() {
//int16_t i;
  wdt_disable();

  auvlibInit();
  sei();

  // Kill switch read
  DDRL &=~_BV(PL0);
  // Kill relay write
  DDRL |= _BV(PL1);

/*
	// Set up Power inhibit lines
	DDRC |= _BV(PC0) | _BV(PC1);
	// Start out with Camera power enabled:
	PORTC|= _BV(PC1);
*/ // ToDo:  determine which pin.

  DeadManCounter = 0;
  DeadManTimeout = 120;
  timer2SetPrescaler(TIMER_CLK_DIV256);
  timerAttach( TIMER2OVERFLOW_INT, DeadManTimer );

  MarkerDropperInit();
  MotorControllersInit();
  a2dStuffInit();

  setKillMotors(1);

//	wdt_enable( 5 );

  // uncomment this when other stuff is verified to be working:
  a2dStartConvert();

  while(1) {
    // If we receive any message, reset the dead man counter
	if( auvlibProcess() ) {
	  DeadManCounter = 0;
	}

	// Motor refresh block, should run at around 50Hz
    if( mcRefresh ) {

      updateMD22(I2C_ADDR_VERT, vertical);
      updateMD22(I2C_ADDR_HORZ, horizontal);
      updateMD22(I2C_ADDR_DRIV, drive);

      mcRefresh = 0;
    }

  }

  return 1;
}



////////////////////////////////////////////////////////////////////
// More ICD Stuff

u08 killsw;

// Returns size of payload, if TxIgnore not set.  Must return 0 if TxBusy||TxNotImpl||TxIgnore.
// TxBusy,TxNotImpl should not be set if TxIgnore is set.
u08 auvlibMsgHandler(u08 type, u08 len, u08 const *data) {
  u08 i;
  TxIgnore=1;           // assume no ack wanted.
  TxBusy=TxNotImpl = 0; // assume neither is true.
  switch(type) {
    ////////////////////////////////////////// BASE MESSAGES
    case 0x01: // processor type
      if(len!=0)
        return 0;
	  TxPacket[0]=0x20;
#if   defined(__AVR_AT90CAN128__)
		TxPacket[1]=0;
#elif defined(__AVR_ATmega2561__) || defined(__AVR_ATmega2560__)
		TxPacket[1]=1;
#elif defined(__AVR_ATmega644__ )
		TxPacket[1]=2;
#else
#error Unknown MCU
#endif
	  TxIgnore = 0;
	  return 2;    // 2 byte payload
    case 0x02: // get software version
	  if(len!=0)
	    return 0;
      *(u16 *)(TxPacket+0) = VERSION_MAJOR;
      *(u16 *)(TxPacket+2) = VERSION_MINOR;
      *(u16 *)(TxPacket+4) = VERSION_MICRO;
	  TxIgnore = 0;
	  return 6;    // 6 byte payload
    case 0x03: // processor description
	  if(len!=0)
	    return 0;
      for(i=0; szProcDesc[i]; i++)
	    TxPacket[i]=szProcDesc[i]; // don't copy terminator.
      TxIgnore = 0;
	  return i;    // strlen(szProcDesc) byte payload
    case 0x04: // max payload
	  if(len!=0)
	    return 0;
      TxPacket[0]=RX_PACKET_SIZE;
	  TxPacket[1]=sizeof(TxPacket);
	  TxIgnore = 0;
	  return 2;    // 2 byte payload
    case 0x05: // board function
	  if(len!=0)
	    return 0;
      TxPacket[0]=0x04; // mixed set #1
	  TxIgnore = 0;
	  return 1;    // 1 byte payload
    case 0x06: // board serial number
	  if(len!=0)
	    return 0;
      TxPacket[0]=szBoardSerno[0];
      TxPacket[1]=szBoardSerno[1];
      TxPacket[2]=szBoardSerno[2];
      TxPacket[3]=szBoardSerno[3];
      TxPacket[4]=szBoardSerno[4];
      TxPacket[5]=szBoardSerno[5];
	  TxIgnore = 0;
	  return 6;    // 6 byte payload
    case 0x07: // board description
	  if(len!=0)
	    return 0;
      for(i=0; szBoardDesc[i]; i++)
	    TxPacket[i]=szBoardDesc[i]; // don't copy terminator.
      TxIgnore = 0;
	  return i;    // strlen(szBoardDesc) byte payload
    case 0x08: // board status
	  if(len!=0)
	    return 0;
      TxPacket[0] = ucStatus;
	  TxIgnore = 0;
	  return 1;    // 1 byte payload

    ////////////////////////////////////////// STANDARD MESSAGES
    case 0x10: // board voltage
	  if(len!=0)
	    return 0;
      *(u32 *)(TxPacket+0) = *(u32 *)&fVoltage;
	  TxIgnore = 0;
	  return 4;    // 4 byte payload
    case 0x11: // board current
	  if(len!=0)
	    return 0;
      *(u32 *)(TxPacket+0) = *(u32 *)&fCurrent;
	  TxIgnore = 0;
	  return 4;    // 4 byte payload
    case 0x12: // board reset (comms IC)
    case 0x13: // MCU   reset
	  if(len!=0)
	    return 0;
      TxNotImpl = 1;
	  TxIgnore  = 0;
	  return 0;    // 0 byte payload
    case 0x14: // status storage
	  if(len!=1 || data[0]>0x02)
	    return 0;
      TxNotImpl = 1;
	  TxIgnore  = 0;
	  return 0;    // 0 byte payload
    
    ////////////////////////////////////////// BOARD-SPECIFIC MESSAGES
    // ToDo: figure out what to do with "present".



	case 0x20: // set motor PWM
      if(len!=6)
        return 0;
      vertical  [0] = data[0];
      vertical  [1] = data[1];
      horizontal[0] = data[2];
      horizontal[1] = data[3];
      drive     [0] = data[4];
      drive     [1] = data[5];

      if( stop )
        stop = 0;

      mcRefresh = 1;

      TxPacket[0] = 0; // just say we succeeded for now
      *(u16 *)(TxPacket+1) = i2cresetcnt;
	  TxIgnore = 0;
      return 3; // 3 byte payload
	case 0x21: // sensor request
      if(len!=0)
	    return 0;

      if( stop ) { // If deadman kicked in...
	    // With new custom boards, we still need to control the kill relay, even
		//   though motor commands are no longer sent.  Sensor messages are always
		//   sent, so this is the least-problematic message to rely on.
		// But, just in case MD22's are attached, we still want to zero them if
		//   we have not gotten a motor command recently.
		// In reality, this only acts the first time (in awhile) we get a message
		//   of any sort, since any valid message resets the deadman, but does 
		//   not reset "stop".
		vertical  [0] = vertical  [1] = 0;
		horizontal[0] = horizontal[1] = 0;
		drive     [0] = drive     [1] = 0;
        stop = 0; // reset "stop", which causes relay to enable.

        mcRefresh = 1;
	  }

      // compute switch(es)
      killsw = (PINL&_BV(PL0)) ? 0x80 : 0; // kill switch

      *(u16 *)(TxPacket+0)=0/*comphead*/;
      TxPacket[2]=(u08)(g_nA2DData[1]>>2); // discard two LSBs (first depth)
      TxPacket[3]=(u08)(g_nA2DData[0]>>2); // discard two LSBs (battery)
      TxPacket[4]=killsw | (u08)(g_nA2DData[1]&3) | (((u08)(g_nA2DData[0]&3))<<2); // insert two LSBs

// battery level        -> g_nA2DData[0]
// (first) depth sensor -> g_nA2DData[1]

      TxIgnore = 0;
      return 5; // 5 byte payload
	case 0x22: // marker dropper command
      if(len!=2)
        return 0;
      if(*(u16*)data< 600 || *(u16*)data> 2000)
        return 0;
      SetMarkerPosition(*(u16*)data);
      TxIgnore = 0;
      return 0; // 0 byte payload
    case 0x23: // power command
      if(len!=1)
	    return 0;

/*    if(data[0]&0x10) { // enforce corresponding low bit?
        if(data[0]&0x01)
          sbi(PORTC, PC0);
        else
          cbi(PORTC, PC0);
      }
      if(data[0]&0x20) { // enforce corresponding low bit?
        if(data[0]&0x02)
          sbi(PORTC, PC1);
        else
          cbi(PORTC, PC1);
      }
      // yes, we really want PORTC here, since we are just checking the current OUTPUT state.
      TxPacket[0]=((PORTC&_BV(PC0))?0x01:0x00) | ((PORTC&_BV(PC1))?0x02:0x00);
*/ 
      TxPacket[0]=0; // ToDo:  determine which pin.
      TxIgnore = 0;
      return 1; // 1 byte payload
    default: // unknown
      return 0;
  }
}







////////////////////////////////////////////////////////////////////
// More Motor Stuff

void updateMD22( u08 addr, s08* channels ) {
    u08 retval;
    s08 speed_cmd[3];

    speed_cmd[0] = 1;       // Set Speed Register

    // If the stop condition is active, set speed to 0
    if( stop ) {
        speed_cmd[1] = 0;
        speed_cmd[2] = 0;
        setKillMotors(1);
    } else {
        //Scale down to avoid killing motors
        // TODO - make this suck less
        speed_cmd[1] = channels[0] / 2;
        speed_cmd[2] = channels[1] / 2 ;
        setKillMotors(0);
    }

    //Threshold the speed command so we can't damage thrusters
    //TODO find the magic power level

//	setRed(1);
    retval = i2cMasterSendNI(addr, 3, (u08 *)speed_cmd);
//	setRed(0);

	if(retval == I2C_DIDRESET)
		i2cresetcnt++;

    //If communication to the motor controller failed
    //  we need to turn off the motors
//    if( retval != I2C_OK )
//        setRed(1);
//    else
//        setRed(0);
}


// Initialize the motor controllers over i2c, displaying
// any errors on the status lights

void MotorControllersInit(void) {
    u08 retval[3];
    u08 txData[5];

    //      set local device address and allow response to general call
//  i2cSetLocalDeviceAddr(0x90, TRUE);

    //      Configuration settings for the motor controllers should be loaded
    //      here.

    txData[0] = 0; // select register 0
    txData[1] = 0x01; // select mode 1: direct control, signed bytes
    txData[2] = 0x00; // both speeds in neutral
    txData[3] = 0x00;
    txData[4] = 0x50; // acceleration is 0x50

    timerPause(500);

    retval[0] = i2cMasterSendNI(I2C_ADDR_VERT, 2/*5*/, txData);
    retval[1] = i2cMasterSendNI(I2C_ADDR_HORZ, 2/*5*/, txData);
    retval[2] = i2cMasterSendNI(I2C_ADDR_DRIV, 2/*5*/, txData);

    // For each motor controller's response, display it on the status lights
	setRed(0);
	setGreen(0);
	timerPause(400);

    int i = 0;
    for( i = 0; i < 3; i++ ) {
        if( retval[i] != I2C_OK ) setRed(1);
        else setGreen(1);
        timerPause(100);
		setRed(0);
        setGreen(0);
        timerPause(400);
    }
	if((retval[0]!=I2C_OK)||(retval[1]!=I2C_OK)||(retval[2]!=I2C_OK)) {
		present&=~0x80;
//		present|= 0x80;              // ************** COMPASS-ONLY TESTING *****************
//		setRed(1);
	} else {
		present|= 0x80;
	}
}


//Function to set the kill relay.  0 is closed (motors on),
// 1 is open (motors off)
u08 setKillMotors(u08 state) {
    //For 0, set PL1 (run motors), for 1 clear PL1
    if( state )
        PORTL &= ~(_BV(PL1));
    else
        PORTL |= _BV(PL1);
    return state;
}
